#ifndef GRBL_MASTER_HPP
#define GRBL_MASTER_HPP

#include <string>
#include <vector>
//#include "../../../Comm/SerialPortManager.hpp"
//#include "../SerialPortManager/SerialPortManager.hpp"

typedef void(__stdcall* PlatformLogCallback)(std::vector<uint8_t> log);

#define GRBL_ALARM 'A'
#define GRBL_ALARM_TRIGGERED 'T'
#define GRBL_UNDEFINED 'U'
#define GRBL_IDLE 'I'
#define GRBL_RUN 'R'
#define GRBL_HOME 'H'

// https://wiki.shapeoko.com/index.php/G-Code
// unit: mm
// notes on hard limit:
// to enable hard limit:
//   send "$21=1" (hard limits)
// when triggered, it produces: Alarm: Hard limit...Reset to continue...
// and does not respond to anything
// you must send 24/0x18 (ctrl-x) to reset then $x to unlock
// staying in trigger position won't cause another trigger. But your jittering may!

// to enable homing: see https://github.com/gnea/grbl/wiki/Set-up-the-Homing-Cycle
// send "$22=1"
// set homing direction

// switching/setting coordinates
// we use g54 as our "global" coordinate (0,0,0 at origin)
// g55~g59 for slide #0~#4
// however, during scan, we simply have g92 (temp offset), so we never sets g55~g59 for now
// https://www.cnccookbook.com/g54-g92-g52-work-offsets-cnc-g-code/

// notes on commands:
// ctrl-? see https://blog.csdn.net/as3522/article/details/102972495
struct GRBLParser {
	// do some processing like add an ending
	static void sharedCmdProc(std::vector<uint8_t>& cmd);

	static std::vector<uint8_t> noCmd();

	static std::vector<uint8_t> getReplyDelim();

	static std::vector<uint8_t> getCmdBuildInfo();
	static std::vector<uint8_t> getCmdReset();
	//static std::vector<uint8_t> getReplyResetLegacy();
	static std::vector<uint8_t> getReplyReset();
	static std::vector<uint8_t> getCmdStateQuarry();
	static std::vector<uint8_t> getCmdConfigQuarry();
	static std::vector<uint8_t> getCmdSetConfig(std::string codeStr, std::string valStr);
	static std::vector<uint8_t> getCmdCoordOffsetsQuarry();

	static std::vector<uint8_t> getCmdSetXMaxSpeed(float mmPerMin);
	static std::vector<uint8_t> getCmdSetYMaxSpeed(float mmPerMin);
	static std::vector<uint8_t> getCmdSetZMaxSpeed(float mmPerMin);

	static std::vector<uint8_t> getCmdSetXMaxAcc(float mmPerSecSq);
	static std::vector<uint8_t> getCmdSetYMaxAcc(float mmPerSecSq);
	static std::vector<uint8_t> getCmdSetZMaxAcc(float mmPerSecSq);

	// rarely used
	//static std::vector<uint8_t> getCmdGoToMachinePos(
	//	bool flagMovX, float movXDist,
	//	bool flagMovY, float movYDist,
	//	bool flagMovZ, float movZDist);
	// set temporary origin, can be removed by command
	static std::vector<uint8_t> getCmdSetWorkCoordTempOrigin();
	// set temporary position, can be removed by command
	static std::vector<uint8_t> getCmdSetWorkCoordTempPosition(float x, float y, float z);
	static std::vector<uint8_t> getCmdRemoveWorkCoordTempOrigin();
	// go to absolute position
	static std::vector<uint8_t> getCmdGoToWorkPos(
		int axisIdx, float dist);
	static std::vector<uint8_t> getCmdGoToWorkPos(
		bool flagMovX, float movXDist,
		bool flagMovY, float movYDist,
		bool flagMovZ, float movZDist);
	static std::vector<uint8_t> getCmdGoToWorkPos(
		std::vector<float> dists, int mask);
	static std::vector<uint8_t> getCmdGoToWorkPosAtRate(
		std::vector<float> dists, int mask,
		float mmPerMin);
	//static std::vector<uint8_t> getCmdGoToWorkPos(
	//	float movXDist,
	//	float movYDist,
	//	float movZDist,
	//	float movADist,
	//	float movBDist,
	//	float movCDist, int mask);
	// go to relative position
	// to rename
	static std::vector<uint8_t> getCmdMoveDistance(
		int axisIdx, float dist);
	static std::vector<uint8_t> getCmdMoveDistance(
		bool flagMovX, float movXDist,
		bool flagMovY, float movYDist,
		bool flagMovZ, float movZDist);
	static std::vector<uint8_t> getCmdMoveDistance(
		std::vector<float> dists, int mask);
	//static std::vector<uint8_t> getCmdGoToRltPos(
	//	float movXDist,
	//	float movYDist,
	//	float movZDist,
	//	float movADist,
	//	float movBDist,
	//	float movCDist, int mask);
	//static std::vector<uint8_t> getCmdGoToRltPosAtRate(
	//	float movXDist,
	//	float movYDist,
	//	float movZDist,
	//	float movADist,
	//	float movBDist,
	//	float movCDist, int mask,
	//	float mmPerMin);
	static std::vector<uint8_t> getCmdMoveDistanceAtRate(
		int axisIdx, int mask, float mmPerMin);
	static std::vector<uint8_t> getCmdMoveDistanceAtRate(
		std::vector<float> dists, int mask, float mmPerMin);
	static std::vector<uint8_t> getGeneralReply();
	// special ones like $110=...
	static std::vector<uint8_t> getConfigReply();

	static std::vector<uint8_t> getCmdStop();
	static std::vector<uint8_t> getReplyStop();
	// should soft reboot to allow unlock
	static std::vector<uint8_t> getCmdUnlock();
	static std::vector<uint8_t> getReplyUnlocked();
	// when limit is touched with hardlimit enabled, the controller will be locked and send a message
	static std::vector<uint8_t> getNoteToUnLock();
	static std::vector<uint8_t> getReplyHardLimitedIdle();
	static std::vector<uint8_t> getReplyHardLimitedRun();
	//static std::vector<uint8_t> getReplyHardLimited1();
	static std::vector<uint8_t> getCmdEnableHardLimit();
	static std::vector<uint8_t> getCmdDisableHardLimit();

	// must enable to do it. disable it after use or you will be forced to do it upon boot everytime(?)
	static std::vector<uint8_t> getCmdEnableHoming(bool flagEnable);
	// our standard coordinate: x+ right, y+ down, z+ lower
	static std::vector<uint8_t> getCmdSetHomingDirection(int xDir, int yDir, int zDir);
	// the slower speed towards home
	static std::vector<uint8_t> getCmdSetHomingFeedSpeed(int feedMMPerMin);
	// the faster speed towards home
	static std::vector<uint8_t> getCmdSetHomingSeekSpeed(int seekMMPerMin);
	// get going. can be stopped by stop command
	static std::vector<uint8_t> getCmdStartHoming(int axis, uint32_t fullBitMask);

	// #-1 for our custom global position
	// #0~#2 for each slide
	// for grbl, P0=current, P1=G54 (???),...
	static std::vector<uint8_t> getCmdSetCoord(int coordIdx, float x, float y);
	static std::vector<uint8_t> getCmdSetCoord(int coordIdx, float x, float y, float z, float a, float b, float c, int mask);
	// #-1 for our custom global position
	// #0~#2 for each slide
	static std::vector<uint8_t> getCmdSwitchToCoord(int coordIdx);

	static std::vector<uint8_t> getCmdPrintParameters(int coordIdx);

	static bool parseIsIdle(std::vector<uint8_t> replyIn);
	static bool parseIsMoving(std::vector<uint8_t> replyIn);
	static bool parseIsAlarm(std::vector<uint8_t> replyIn);
	static bool parseIsHoming(std::vector<uint8_t> replyIn);

	// or global position
	static std::vector<float> parseMachinePos(std::vector<uint8_t> replyIn);
	// or local position (after setting a local origin)
	static std::vector<float> parseWorkPos(std::vector<uint8_t> replyIn);
	static std::vector<int> parseMissingAxises(std::vector<uint8_t> replyIn);

	//static std::vector<int> parseHardLimitLegacy0901(std::vector<uint8_t> replyIn);
	static std::vector<int> parseHardLimit(std::vector<uint8_t> replyIn);
	static int parseHardLimitInt(std::vector<uint8_t> replyIn);
	static std::string parseSensors(std::vector<uint8_t> replyIn);

	static std::string toDisplayable(std::vector<uint8_t>& content, char delim = '\0');

	static std::string translateCoordIndex2Gxx(int coordIdx);

	static void insertString(std::vector<uint8_t>& cmdInOut, std::string cmd);

private:
	const static std::string axisCodes;

	const static std::string paraAbsPosMode;
	const static std::string paraRltPosMode;
	const static std::string paraMove;
	const static std::string paraMoveWithSpeed;
	const static std::string paraMoveSpeedOverwrite;
	const static std::string paraSetTempWorkPos;
	const static std::string paraRemoveTempWorkPos;
	const static std::string paraEnableHardLimit;
	const static std::string paraDisableHardLimit;
	const static std::string paraEnableHoming;
	const static std::string paraStartHoming;
	const static std::string paraDisableHoming;
	const static std::string paraSetOffset;
	const static std::string paraSetXMaxSpeed;
	const static std::string paraSetYMaxSpeed;
	const static std::string paraSetZMaxSpeed;
	const static std::string paraSetXMaxAcc;
	const static std::string paraSetYMaxAcc;
	const static std::string paraSetZMaxAcc;
	const static std::string paraEnableHardLimitReport;

	const static std::string stateMove;
	const static std::string stateIdle;
	const static std::string stateAlarm;
	const static std::string stateHoming;
	const static std::string stateCmdReply;

	const static std::string keyMachinePos;
	const static std::string keyWorkPos;
	const static std::string keyLimit;
	const static std::string keyToUnlock;
	const static std::string keyHardLimitedIdle;
	const static std::string keyHardLimitedRun;
	// will receive this on hard limit hit; legacy
	//const static std::string keyHardLimited0;
	// will receive this after sending something else after receiving keyHardLimited0; legacy
	//const static std::string keyHardLimited1;
	const static std::string keyUnlock;

	//static void makePosCore(std::vector<uint8_t>& cmdInOut,
	//	bool flagMovX, float movXDist,
	//	bool flagMovY, float movYDist,
	//	bool flagMovZ, float movZDist);
	//static void makePosCore(std::vector<uint8_t>& cmdInOut,
	//	float movXDist,
	//	float movYDist,
	//	float movZDist,
	//	float movADist,
	//	float movBDist,
	//	float movCDist, int mask);
	// to rename
	static void makePosCore(std::vector<uint8_t>& cmdInOut,
		int axisIdx, float position);
	static void makePosCore(std::vector<uint8_t>& cmdInOut,
		int axisNum, float* movXYZABC, int mask);
	static std::vector<float> parsePosCore(std::vector<uint8_t>& cmdInOut, size_t offset);
	static std::vector<int> parseMissingAxisCore(std::vector<uint8_t>& cmdInOut, size_t offset);

	// use with caution. not all ascii code can be represented as character. 
	static std::string cmd2str(std::vector<uint8_t> content);
};

struct GRBLMaster {
	GRBLMaster();
	~GRBLMaster();
	// take care of connect/disconnect out side
	void init();
	void purge();

	// such as with ESP-NOW, we may receive accident replies such as failure to send, which is followed by the correct reply
	void setReceiveAttemptAllowance(int number);

	bool isSimulating();

	void setPlatformCommFilterActivate(bool newFlag);
	bool canControlPlatform();
	//bool checkPlatformHardLimitedLegacy();
	// this checks if locked, then go through the unlocking API
	//int unlockFromLimitLegacy();
	bool softRebootPlatform();
	// updating config ($??=?) takes effetect after reboot
	bool hardRebootPlatform();
	bool unlockPlatform();
	// bool resetPlatformFromLock();
	bool quickPlatformTest();
	bool setFlagHardLimit(bool flagIn);
	bool updateConfigFromSlave();

	// 110,111,112, default=500mm/min
	// careful: machine will reply 2 ok 
	bool setXMaxSpeed(float mmPerMin);
	bool setYMaxSpeed(float mmPerMin);
	bool setZMaxSpeed(float mmPerMin);

	bool setXMaxAcc(float mmPerMin);
	bool setYMaxAcc(float mmPerMin);
	bool setZMaxAcc(float mmPerMin);

	// bool waitUntilNotBuzy(int timeout);

	// g92
	// after switching coords, this is kept!
	// https://tormach.com/temporary-work-offsets-g92-g92-1-g92-2-and-g92-3
	bool setWorkPositionTempOrigin();
	bool setWorkPositionTempPosition(float x, float y, float z);
	bool removeWorkPositionTempOrigin();
	//bool mov2WorkPosAbsMasked(float* xyzabc, int mask, float rateMMPerMin);

	/*
	Rarely used move commands
	*/
	bool mov2WorkPosAbs(float x, float y);
	bool mov2WorkPosAbs(float x, float y, float z);
	// move platform (+ is right)
	bool movHrz(float cmdMM);

	// move platform (+ is down)
	bool movVtc(float cmdMM);

	bool movPlatform(float cmdHrzMM, float cmdVtcMM);
	bool movPlatform(float cmdHrzMM, float cmdVtcMM, float cmdPerpenMM);

	bool movPlatformEx(char exAxisName, float cmdMM);

	// obsolete. use movAxisesByDist
	//bool movByDistMasked(float* xyzabc, int mask);


	//bool movPlatformA(float cmdMM);
	//bool movPlatformB(float cmdMM);
	//bool movPlatformC(float cmdMM);
	//bool movABC(int axisIdx, float cmdMM);

	/*
	General move commands
	*/
	// single axis, relative distance
	bool movAxisByDist(int axisIdx, float distance, float mmPerMin = -1);
	// multiple axises defined by mask, relative distances
	bool movAxisesByDist(int axisMask, std::vector<float> distances, float mmPerMin = -1);
	// move 2 direction at once, so you retrace both directions

	// multiple axises defined by mask, absolute work position
	bool movAxisesToPosition(int axisMask, std::vector<float> positions, float mmPerMin = -1);
	bool movAxisesToTargetPosition(int axisMask, float mmPerMin = -1);
	// for buffering command
	std::vector<uint8_t> movAxisesToPositionBuildCmd(int axisMask, std::vector<float> positions, float mmPerMin = -1);

	int stopPlatform();
	bool isPlatformStopped(int delayMSAfterMov);
	bool waitUntilPlatformStopIfNeeded(bool flagWaitUntilStop, int delayMSAfterMov);

	// move lens (+ is lower)
	bool movPlatformPerpendicular(float cmdMM);
	bool movPlatformPerpendicularAbs(float cmdMM);

	int simuUpdate();
	bool activateAxises(int axisMask, int axisNum);

	// full version
	bool getPlatformStateQuarry();
	//bool getPlatformStateReceive(std::string* stateToPrint, CtrlMovState* ctrlStateOut,
	//	float* machinePosXOut, float* machinePosYOut, float* machinePosZOut, float* machinePosAOut,
	//	float* workPosXOut, float* workPosYOut, float* workPosZOut, float* workPosAOut,
	//	int* xLimOut, int* yLimOut, int* zLimOut);
	//// machine pos is persistent
	//// origin of work pos may be changed
	//bool getPlatformState(std::string* stateToPrint, CtrlMovState* ctrlStateOut,
	//	float* machinePosXOut, float* machinePosYOut, float* machinePosZOut, float* machinePosAOut,
	//	float* workPosXOut, float* workPosYOut, float* workPosZOut, float* workPosAOut,
	//	int* xLimOut, int* yLimOut, int* zLimOut);

	// compact version
	bool getPlatformStateReceive(char* stateOut,
		int coordNum, float* workPosesOut,
		int* allLimOut);
	bool getPlatformState(char* stateOut = nullptr,
		int coordNum = 0, float* workPosesOut = nullptr,
		int* allLimOut = nullptr);

	bool getPlatformStateReceiveCore();

	// enable homing -> "$22=1"
	// set feed speed (fast) if needed ->  $25=?f, default: 500
	// set seek speed (slow) if needed -> $24=?f, default: 25
	// from https://github.com/gnea/grbl/wiki/Set-up-the-Homing-Cycle
	bool startHoming(int axisMask);
	bool isHomingCompleteLagacy();
	// return true if finished, false if not yet
	bool finishHoming();
	// only the homing command
	std::vector<uint8_t> startHomingBuildCmd(int axisMask);
	int confirmPosition(int positionMask, std::vector<float> coordinates, float errorAllowance);

	// switching/setting coordinates
	// we use g54 as our "global" coordinate (0,0,0 at origin)
	// g55~g59 for slide #0~#4
	// however, during scan, we simply have g92 (temp offset), so we never sets g55~g59 for now
	// https://www.cnccookbook.com/g54-g92-g52-work-offsets-cnc-g-code/
	bool switchToCoordSystem(int coordIdx);
	//bool setCoordPosition(int coordIdx, float curX, float curY);
	//bool setCoordPosition(int coordIdx, float curX, float curY, float curZ);
	// works but cannot switch coordinate
	bool setCoordPosition(int coordIdx,
		float curX,
		float curY,
		float curZ,
		float curA,
		float curB,
		float curC,
		int mask);

	time_t getPlatformUnavailableDuration();

	void takeControl();
	void releaseControl();

	struct Core {
		void* slaveNativeHandle = nullptr;
		int isHandleOwner = 1;
		std::string slaveManagerCommBuf;

		// not used for now, handled by SerialPortManager
		PlatformLogCallback sentCB = NULL;
		PlatformLogCallback replyCB = NULL;
		PlatformLogCallback timeoutCB = NULL;

		//bool flagSimuMode;
		int platformCoordSystemIdx;

		~Core();

		int getCoordSystemIdx();
		bool sendCmdCore(std::vector<uint8_t> cmd);
		std::vector<uint8_t> peekRply();
		std::vector<uint8_t> receiveRplyCore(std::vector<uint8_t> expReply, int timeout = -1, std::string discardErrorHint = "");
		std::vector<uint8_t> receiveRplyCore(std::string expReply, int timeout = -1);
		bool sendCmdCheckReply(std::vector<uint8_t> cmd, int timeout, std::vector<uint8_t> expectedReply, std::string discardErrorHint = "");
		// special case. it starts and ends with delim
		bool sendResetCmdCheckReply(bool flagInit, int timeout);
		bool sendMovCheckReply(std::vector<uint8_t> cmd);

		bool simuSend(std::vector<uint8_t> cmd);
		bool simuReceive(std::string reply);
	}_core;

	struct State {
		const static int MaxAxNum = 8;
		const static uint32_t FullAxMask = 0b11111111;
		std::string stateToPrint;
		char action = 'I';
		float machPosXYZABC[MaxAxNum] = { 0 };
		float workPosXYZABC[MaxAxNum] = { 0 };
		float targetXYZABC[MaxAxNum] = { 0 };
		float lastIdleXYZABC[MaxAxNum] = { 0 };
		float progress = 0;
		int limits = 0;
		std::string sensors;
		int simuHomingEndTime = 0;
		int simuHomeMask = 0;
		long updateTime = 0;
		int parsedAxisNum = 0;

		void homed();
		std::vector<float> getTarget();
	}_state;

	struct Flags {
		int simuMode = 0;
		// accelerate rate, to make things faster on larger scale
		// set dynamically if range is available
		float simuAccMMPerSecSec = 1;
		// current rate
		float simuRateMMPerSecX = 0;
		float rateMultiplier = 1;
		bool flagBuzy = false;
	
		int receiveAttemptAllowance = 1;
	}_flags;

	// https://stadtfabrikanten.org/display/SF/Grbl+v1.1+Configuration+Manual
	struct PlatformConfig {
		PlatformConfig() {};
		PlatformConfig(std::vector<std::string>& paraCodesIn, std::vector<std::string>& paraValsIn) { setConfig(paraCodesIn, paraValsIn); };
		bool getConfig(std::vector<std::string>& paraCodesOut, std::vector<std::string>& paraValsOut);
		// if not match, this config will be updated assuming you will set it
		bool matchConfigNSetIfNot(std::vector<uint8_t> cmd);
		// if not match, this config will be updated assuming you will set it
		bool matchCoordNSetIfNot(int coordIdx, float xOffset, float yOffset);
		// if not match, this config will be updated assuming you will set it
		bool matchCoordNSetIfNot(int coordIdx, float xOffset, float yOffset, float zOffset);
		// from grbl controller
		void setConfig(bool flagHintIncluded, std::vector<uint8_t> reply);
		// from parsed config text
		void setConfig(std::vector<std::string>& paraCodesIn, std::vector<std::string>& paraValsIn);
		static int difference(PlatformConfig& currentConfig, PlatformConfig& targetConfig, PlatformConfig& changesToMatchTargetReference);
		void setCoordOffsets(std::vector<uint8_t> reply);
		void makeDefaultConfigList0901(std::vector<std::string>& paraCodes, std::vector<std::string>& paraVals);
		void makeDefaultConfigList1101(std::vector<std::string>& paraCodes, std::vector<std::string>& paraVals);

		bool hasEntry(int codeIn);

		std::vector<std::vector<uint8_t>> getCmdToSetToDefaults();
		std::string toStr();
		void reset();
	public:
		std::vector<int> paraCodes;
		//std::vector<float> paraVals;
		std::vector<std::string> paraCodeStrs;
		std::vector<std::string> paraValStrs;
		std::vector<std::string> paraNotes;

		std::vector<std::string> coordNames;
		std::vector<std::vector<float>> coordOffsets;
	}_platformSlaveConfig;

	//private:
	const static int platformResetReplyTimeOut;
	const static int platformNoteToResetTimeout;
	const static int platformGeneralCmdReplyTimeOut;
	const static int platformConfigReplyTimeOut;
	const static int goToCmdReplyTimeOut;
	const static int stateQuarryReplyTimeOut;
	const static int stopTimeOut;
};
#endif